/********** Library Includes **************************************************/

#include <opencv/cv.h>
#include <opencv/highgui.h>

/********** Local Includes ****************************************************/

#include "cannedsource.h"
#include "objectdetector.h"
#include "approxDecomp.h"
#include "common.h"
#include "celldecompastar.h"
#include "voronoi.h"

/********** Macros and Constants **********************************************/

#define IMAGE_FOLDER "real_images3/"
#define IMAGE_EXT ".jpeg"
#define IMAGE_COUNT 2
#define DEBUG true

/********** Namespaces ********************************************************/

using namespace cv;
using namespace std;

/********** Local Functions ***************************************************/

string filename(string image, int i = -1) {
	stringstream ss;
	ss << IMAGE_FOLDER << image;
	
	if (i >= 0) {
		ss << i;
	}
	
	ss << IMAGE_EXT;
	return ss.str();
}

Point robot(231, 241), fruit(662, 241);

void on_mouse(int event, int x, int y, int flags, void *)
{
	static int count = 0;
	
	switch(event) {
		case CV_EVENT_LBUTTONDOWN:
			if (count == 0) {
				robot = Point(x, y);
				DUMP(robot.x);
				DUMP(robot.y);
			} else if (count == 1) {
				fruit = Point(x, y);
				DUMP(fruit.x);
				DUMP(fruit.y);
			}
			
			count++;
			break;
	}
}

void showPlan(const Mat &image, const vector<Point> plan, int blockSize) {
	Mat temp = image.clone();
	
	for (vector<Point>::const_iterator it = plan.begin(); it != plan.end(); ++it) {
		Point p(it->x * blockSize + blockSize / 2,
			it->y * blockSize + blockSize / 2);
		circle(temp, p, 5, CV_RGB(255, 0, 0), CV_FILLED);
		
		vector<Point>::const_iterator next = it + 1;
		
		if (next != plan.end()) {
			Point a(it->x * blockSize + blockSize / 2,
				it->y * blockSize + blockSize / 2);
			Point b(next->x * blockSize + blockSize / 2,
				next->y * blockSize + blockSize / 2);
				
			line(temp, a, b, CV_RGB(255, 0, 0));
		}
	}
	
	imshow("Plan", temp);
	cvMoveWindow("Plan", 0, 900 - temp.size().height);
}

int main(int, char **) {
	cout << "Press 'n' to step forward to the next frame." << endl
		<< "Press 'p' to step back to the previous frame." << endl
		<< "Press 'q' to close." << endl
		<< "In debug view:" << endl
		<< "\tRed is obstacle" << endl
		<< "\tGreen is fruit" << endl
		<< "\tBlue is robot" << endl;
	
	CannedSource *source = new CannedSource();
	ObjectDetector detector(source);
    
	source->setImageFilename(filename("background"));
	detector.saveBackground();
    
	imshow("Processed", detector.processed());
	imshow("Raw", source->next());
	
	cvMoveWindow("Processed", 0, 0);
	cvMoveWindow("Raw", source->next().size().width, 0);
	
	while (cvWaitKey(300) != 'n');
	
	source->setImageFilename(filename("obstacles"));
	detector.saveObstacles();
	
	imshow("Processed", detector.processed());
	imshow("Raw", source->next());
	
	while (cvWaitKey(300) != 'n');
	
	source->setImageFilename(filename("robot_and_fruit", 0));
	imshow("Live", source->next());
	cvSetMouseCallback("Live", on_mouse, 0);
	
	cout << "Click near the robot and fruit (in that order), then press 'c'"
		<< endl;
	
	int key;
	
	while (true) {
		key = waitKey(300);
		
		if (key == 'q') {
			return 0;
		} else if (key == 'c') {
			break;
		}
	}
	
	cvDestroyWindow("Live");
	
	if (DEBUG) {
		cvNamedWindow("Robot Blobs");
		cvMoveWindow("Robot Blobs", source->next().size().width,
			900 - source->next().size().height);
	}
	
	detector.saveRobotAndFruit(robot, fruit, DEBUG);
    
    Mat open = detector.getOpen();
    ApproxDecomp decomp(source, open);
   
    Mat grid = decomp.cellDecomp();
    Voronoi vdiag(source, open);
    Mat secondGrid =vdiag.brushFire();
    
    
	CellDecompAStar astar(grid);
	
	imshow("Processed", detector.processed());
	imshow("Raw", source->next());
	
	Point robot = detector.robot().point;
	Point fruit = detector.fruit();
	
	Point start(robot.x / decomp.blockSize(), robot.y / decomp.blockSize());
	Point goal(fruit.x / decomp.blockSize(), fruit.y / decomp.blockSize());
	
	vector<Point> plan = astar.plan(start, goal);
	showPlan(source->next(), plan, decomp.blockSize());
	
	int i = 0;
	
	while (i < IMAGE_COUNT) {
		int key = cvWaitKey(300);
		
		if (key == 'n') {
			i++;
		} else if (key == 'p') {
			i--;
		} else if (key == 'q') {
			break;
		} else {
			continue;
		}
		
		if (i >= IMAGE_COUNT) {
			i = IMAGE_COUNT - 1;
		} else if (i < 0) {
			i = 0;
			continue;
		}
		
		source->setImageFilename(filename("robot_and_fruit", i));
		detector.findRobot(DEBUG);
		
		imshow("Processed", detector.processed());
		imshow("Raw", source->next());
	}
}